#include <iostream>
#include <cstdlib>
#include <time.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <libplayerc++/playerc++.h>
#include "args.h"
#define RAYS 32
#include <time.h>
#include <signal.h>
#include <ctime>	
#include <sys/time.h>
#include "ekfloc.cpp"

using namespace std;
using namespace PlayerCc;
FILE *posoutput;
FILE *laseroutput;
FILE *mapin;
FILE *LOG;

//goto definitions
int N_VIA=5;
int N_giri = 1;
double viapointX[]={3,3,-3,-3,3};
double viapointY[]={1,-2,-2,2,2};
int vp=0;
int ig=1;
bool finish = false;
int debug=0;

int main(int argc, char** argv){

	LOG=fopen("LOG.txt","w");
    posoutput=fopen("Localisation.txt","w");
	

//These are initialised before the infinite loop
PlayerClient robot(gHostname, gPort);	//This is needed in every player client application
Position2dProxy pp(&robot, gIndex);	//This is needed in every player client application
LaserProxy lp(&robot, gIndex);		//This is needed in every player client application
ekfloc ekf;
//EFK SLAM object
double *laserdata;			//Laser data array
double x,y,yaw,oldx,oldy,oldyaw;	//Variables used to find change in displacement d_d and change in orientation d_t
ekf.set();				//EKF SLAM initialisation
    //oldx=ekf.ox;xest.val[1][0]
	//oldy=ekf.oy;
	//oldyaw=ekf.ot;
	oldx=ekf.xest.val[0][0];
	oldy=ekf.xest.val[1][0];
	oldyaw=ekf.xest.val[2][0];
	
//file di log	
fprintf(LOG,"  init x ,y, yaw :%4.3f/%4.3f/%3.2f\n",x,y,yaw*180.0/PI);
printf("  init x ,y, yaw :%4.3f/%4.3f/%3.2f\n",x,y,yaw*180.0/PI);

int i=0;
while(!finish){
//while (debug!=5){
//		debug=debug+1;
	i=i+1;

try{
	
//These are called during the inifinite recursive loop
robot.Read();				//Obtain DR and laser data
x=pp.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
y=pp.GetYPos();
yaw=pp.GetYaw();
	
//printf("  %d iesimo x ,y, yaw :%4.3f/%4.3f/%3.2f\n",i,x,y,yaw*180.0/PI);
fprintf(LOG," %d iesimo x ,y, yaw :%4.3f/%4.3f/%3.2f\n",i,x,y,yaw*180.0/PI);

	
double d_d=sqrt((oldx-x)*(oldx-x)+(oldy-y)*(oldy-y));
double d_t=yaw-oldyaw;
normalise(d_t);				//Force all angles to be between +-180 degrees
			
laserdata=new double[ekf.laserproxies];	//Set blank array equal to number of proxies in the laser
for(int j=0;j<ekf.laserproxies;j++){
	laserdata[j]=lp.GetRange(j);
}

double ltime=lp.GetDataTime();		//Get timestamp for laser
double ptime=pp.GetDataTime();		//Get timestamp for odometer
double lpdiff=abs(ltime-ptime);		//Find laser and odometer timestampt difference
			
ekf.predict(d_d,d_t);			//EKF predict using d_d and d_t

//logdata
fprintf(posoutput,"%f %f %f %f %f %f %f %f\n",ekf.xest.val[0][0],ekf.xest.val[1][0],ekf.xest.val[2][0],x,y,yaw,d_d,d_t);
fprintf(LOG,"stima dello stato - predict: %f %f %f %f %f %f %f %f\n",ekf.xest.val[0][0],ekf.xest.val[1][0],ekf.xest.val[2][0],x,y,yaw,d_d,d_t);
//printf("%d EKF:%4.3f/%4.3f/%3.2f DR:%4.3f/%4.3f/%3.2f LM: %d\n",i,ekf.xest.val[0][0],ekf.xest.val[1][0],ekf.xest.val[2][0]*180.0/PI,x,y,yaw*180.0/PI,(ekf.xest.nRow()-3)/2);
		

ekf.update(laserdata,d_t,lpdiff);	//EKF update using laser data, d_t and lpdiff
fprintf(LOG,"stima dello stato - update: %f %f %f %f %f %f %f %f\n",ekf.xest.val[0][0],ekf.xest.val[1][0],ekf.xest.val[2][0],x,y,yaw,d_d,d_t);

// goto commands . . . . .  . . . .. . . . .  . . . . 
if(N_giri!=0)
if (viapointX[vp] < x+0.2 && viapointX[vp] > x-0.2  && viapointY[vp] < y+0.2 && viapointY[vp] > y-0.2 ){
         vp=vp+1;
		 robot.Read();
         pp.GoTo(viapointX[vp],viapointY[vp],0);
std::cout << "goto starting, target: " << viapointX[vp]
              << ", " << viapointY[vp] << std::endl;
std::cout << "indice target i: " << vp << std::endl;
std::cout << "giro " << ig << std::endl;
}

if(vp>=N_VIA && ig<N_giri){
ig=ig+1;
vp=0;
}

if(vp>=N_VIA && ig>=N_giri){
finish= true;
}

oldx=x;
oldy=y;
oldyaw=yaw;	
}
  catch (PlayerCc::PlayerError & e)
  {
    std::cerr << e << std::endl;
    return -1;
  }

}//THE END loop


fclose(posoutput);
fclose(LOG);

}
	
